%Problem 2c 
kp = 45;
kd = 55;

Gc2c = tf([kd kp], 1);
Gp2c = tf(1, [1 5 4 0]);
GcGp2c = Gc2c * Gp2c;

subplot(3,1,1)
rlocus(Gp2c);
title('Problem 2c: Uncompensated system G_p');

subplot(3,1,2)
rlocus(GcGp2c);
title('Problem 2c: Compensated system G_c(s)G_p(s)');
[y, t] = step(feedback(GcGp2c, 1) * tf(1, [1 0]));

subplot(3,1,3)
step(feedback(GcGp2c, 1) * tf(1, [1 0]));
title('Problem 2c: Ramp response for Compensated system');
index = find(abs(y - t) == max(abs(y-t)));
overshoot = (abs( min(y-t) ) / t(index)) * 100;
ssError = abs(y(end) - t(end));

%Print the performance
disp(['Kp: ', num2str(kp)])
disp(['Kd: ', num2str(kd)])
disp(['Overshoot: ', num2str(overshoot), '%'])
disp(['Steady State Error: ', num2str(ssError)])

%Results in the following output:
%--------------------------------
% Kp: 45
% Kd: 55
% Overshoot: 8.2909%
% Steady State Error: 0.088889
%--------------------------------

%Problem 3a
GcGp3a = tf([1], [1 4 14 20]);
figure;
rlocus(GcGp3a)
title('Problem 3a: Compensated system G_c(s)G_p(s)');

